﻿
using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Runtime.CompilerServices;
using System.Text;
using System.Threading.Tasks;
using System.Transactions;

namespace Meow.Core;

public static class NumericsExts
{
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector4 Column0(in Matrix4x4 mat4) { return new Vector4(mat4.M11, mat4.M21, mat4.M31, mat4.M41); }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector4 Column1(in Matrix4x4 mat4) { return new Vector4(mat4.M12, mat4.M22, mat4.M32, mat4.M42); }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector4 Column2(in Matrix4x4 mat4) { return new Vector4(mat4.M13, mat4.M23, mat4.M33, mat4.M43); }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector4 Column3(in Matrix4x4 mat4) { return new Vector4(mat4.M14, mat4.M24, mat4.M34, mat4.M44); }


    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Column0xyz(in Matrix4x4 mat4) { return new Vector3(mat4.M11, mat4.M21, mat4.M31); }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Column1xyz(in Matrix4x4 mat4) { return new Vector3(mat4.M12, mat4.M22, mat4.M32); }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Column2xyz(in Matrix4x4 mat4) { return new Vector3(mat4.M13, mat4.M23, mat4.M33); }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Column3xyz(in Matrix4x4 mat4) { return new Vector3(mat4.M14, mat4.M24, mat4.M34); }



    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion QuaternionMul(Quaternion q, float x)
    {
        return new Quaternion(x * q.X, x * q.Y, x * q.Z, x * q.W);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion QuaternionMul(float x, Quaternion q)
    {
        return new Quaternion(x * q.X, x * q.Y, x * q.Z, x * q.W);
    }


    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Matrix4x4 Create(in Vector3 pos, in Quaternion rot, in Vector3 scale)
    {
        var mat = Matrix4x4.CreateFromQuaternion(rot) * Matrix4x4.CreateScale(scale);
        mat.M41 = pos.X;
        mat.M42 = pos.Y;
        mat.M43 = pos.Z;
        return mat;
    }

    /// <summary>
    /// Matrix * Vector3, w is 0
    /// Transform a Vector
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static void TransVecRef(in Matrix4x4 mat, ref Vector3 v)
    {
        v.X = mat.M11 * v.X + mat.M21 * v.Y + mat.M31 * v.Z;
        v.Y = mat.M12 * v.X + mat.M22 * v.Y + mat.M32 * v.Z;
        v.Z = mat.M13 * v.X + mat.M23 * v.Y + mat.M33 * v.Z;
    }

    /// <summary>
    /// Matrix * Vector3, w is 0
    /// Transform a Vector
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 TransVec(in Matrix4x4 mat, Vector3 v)
    {
        v.X = mat.M11 * v.X + mat.M21 * v.Y + mat.M31 * v.Z;
        v.Y = mat.M12 * v.X + mat.M22 * v.Y + mat.M32 * v.Z;
        v.Z = mat.M13 * v.X + mat.M23 * v.Y + mat.M33 * v.Z;
        return v;
    }

    /// <summary>
    /// Matrix * Vector3, w is 1
    /// Use For Transform A Point
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static void TransPosRef(in Matrix4x4 mat, ref Vector3 v)
    {
        v.X = mat.M11 * v.X + mat.M21 * v.Y + mat.M31 * v.Z + mat.M41;
        v.Y = mat.M12 * v.X + mat.M22 * v.Y + mat.M32 * v.Z + mat.M42;
        v.Z = mat.M13 * v.X + mat.M23 * v.Y + mat.M33 * v.Z + mat.M43;
    }

    /// <summary>
    /// Matrix * Vector3, w is 1
    /// Use For Transform A Point
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static void TransPosRef(this Matrix4x4 mat, ref Vector3 v)
    {
        v.X = mat.M11 * v.X + mat.M21 * v.Y + mat.M31 * v.Z + mat.M41;
        v.Y = mat.M12 * v.X + mat.M22 * v.Y + mat.M32 * v.Z + mat.M42;
        v.Z = mat.M13 * v.X + mat.M23 * v.Y + mat.M33 * v.Z + mat.M43;
    }

    /// <summary>
    /// Matrix * Vector3, w is 1
    /// Use For Transform A Point
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 TransPos(in Matrix4x4 mat, Vector3 v)
    {
        v.X = mat.M11 * v.X + mat.M21 * v.Y + mat.M31 * v.Z + mat.M41;
        v.Y = mat.M12 * v.X + mat.M22 * v.Y + mat.M32 * v.Z + mat.M42;
        v.Z = mat.M13 * v.X + mat.M23 * v.Y + mat.M33 * v.Z + mat.M43;
        return v;
    }

    /// <summary>
    /// Matrix * Vector3, w is 1
    /// Use For Transform A Point
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 TransPos(this Matrix4x4 mat, Vector3 v)
    {
        v.X = mat.M11 * v.X + mat.M21 * v.Y + mat.M31 * v.Z + mat.M41;
        v.Y = mat.M12 * v.X + mat.M22 * v.Y + mat.M32 * v.Z + mat.M42;
        v.Z = mat.M13 * v.X + mat.M23 * v.Y + mat.M33 * v.Z + mat.M43;
        return v;
    }
    /// <summary>
    /// Matrix * Vector4
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static void Mul(in Matrix4x4 mat, ref Vector4 v)
    {
        v.X = mat.M11 * v.X + mat.M21 * v.Y + mat.M31 * v.Z + mat.M41 * v.W;
        v.Y = mat.M12 * v.X + mat.M22 * v.Y + mat.M32 * v.Z + mat.M42 * v.W;
        v.Z = mat.M13 * v.X + mat.M23 * v.Y + mat.M33 * v.Z + mat.M43 * v.W;
        v.W = mat.M14 * v.X + mat.M24 * v.Y + mat.M34 * v.Z + mat.M44 * v.W;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool Decompose(this Matrix4x4 mat, out Vector3 scale, out Quaternion rotation, out Vector3 translation)
    {
        ExtractTranslate(in mat, out translation);
        ExtractScale(in mat, out scale);
        return ExtractRotation(in mat, out rotation, in scale);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool Decompose(in Matrix4x4 mat, out Vector3 scale, out Quaternion rotation, out Vector3 translation)
    {
        ExtractTranslate(in mat, out translation);
        ExtractScale(in mat, out scale);
        return ExtractRotation(in mat, out rotation, in scale);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static void ExtractTranslate(this Matrix4x4 mat, out Vector3 translation)
    {
        translation.X = mat.M41;
        translation.Y = mat.M42;
        translation.Z = mat.M43;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static void ExtractTranslate(in Matrix4x4 mat, out Vector3 translation)
    {
        translation.X = mat.M41;
        translation.Y = mat.M42;
        translation.Z = mat.M43;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static void ExtractScale(this Matrix4x4 mat, out Vector3 scale)
    {
        float num = ((Math.Sign(mat.M11 * mat.M12 * mat.M13 * mat.M14) >= 0) ? 1 : (-1));
        float num2 = ((Math.Sign(mat.M21 * mat.M22 * mat.M23 * mat.M24) >= 0) ? 1 : (-1));
        float num3 = ((Math.Sign(mat.M31 * mat.M32 * mat.M33 * mat.M34) >= 0) ? 1 : (-1));
        scale.X = num * MathF.Sqrt(mat.M11 * mat.M11 + mat.M12 * mat.M12 + mat.M13 * mat.M13);
        scale.Y = num2 * MathF.Sqrt(mat.M21 * mat.M21 + mat.M22 * mat.M22 + mat.M23 * mat.M23);
        scale.Z = num3 * MathF.Sqrt(mat.M31 * mat.M31 + mat.M32 * mat.M32 + mat.M33 * mat.M33);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static void ExtractScale(in Matrix4x4 mat, out Vector3 scale)
    {
        float num = ((Math.Sign(mat.M11 * mat.M12 * mat.M13 * mat.M14) >= 0) ? 1 : (-1));
        float num2 = ((Math.Sign(mat.M21 * mat.M22 * mat.M23 * mat.M24) >= 0) ? 1 : (-1));
        float num3 = ((Math.Sign(mat.M31 * mat.M32 * mat.M33 * mat.M34) >= 0) ? 1 : (-1));
        scale.X = num * MathF.Sqrt(mat.M11 * mat.M11 + mat.M12 * mat.M12 + mat.M13 * mat.M13);
        scale.Y = num2 * MathF.Sqrt(mat.M21 * mat.M21 + mat.M22 * mat.M22 + mat.M23 * mat.M23);
        scale.Z = num3 * MathF.Sqrt(mat.M31 * mat.M31 + mat.M32 * mat.M32 + mat.M33 * mat.M33);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool ExtractRotation(this Matrix4x4 mat, out Quaternion rotation, in Vector3 scale)
    {
        if ((double)scale.X == 0.0 || (double)scale.Y == 0.0 || (double)scale.Z == 0.0)
        {
            rotation = Quaternion.Identity;
            return false;
        }

        Matrix4x4 matrix = new Matrix4x4(
            mat.M11 / scale.X, mat.M12 / scale.X, mat.M13 / scale.X, 0f,
            mat.M21 / scale.Y, mat.M22 / scale.Y, mat.M23 / scale.Y, 0f,
            mat.M31 / scale.Z, mat.M32 / scale.Z, mat.M33 / scale.Z, 0f,
            0f, 0f, 0f, 1f);

        rotation = Quaternion.CreateFromRotationMatrix(matrix);
        return true;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool ExtractRotation(in Matrix4x4 mat, out Quaternion rotation, in Vector3 scale)
    {
        if ((double)scale.X == 0.0 || (double)scale.Y == 0.0 || (double)scale.Z == 0.0)
        {
            rotation = Quaternion.Identity;
            return false;
        }

        Matrix4x4 matrix = new Matrix4x4(
            mat.M11 / scale.X, mat.M12 / scale.X, mat.M13 / scale.X, 0f,
            mat.M21 / scale.Y, mat.M22 / scale.Y, mat.M23 / scale.Y, 0f,
            mat.M31 / scale.Z, mat.M32 / scale.Z, mat.M33 / scale.Z, 0f,
            0f, 0f, 0f, 1f);

        rotation = Quaternion.CreateFromRotationMatrix(matrix);
        return true;
    }


}
